#include "RobotSport.h"
#include "ui_RobotSport.h"
#include "enumfontsome.h"



RobotSport::RobotSport(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::RobotSport)
{
    ui->setupUi(this);

    MK_DIR("rundata/tubeMove");
    MK_DIR("rundata/mult");
    ui->pbPos->setText(QChar(IconIdentity::icon_location_arrow));
    ui->pbAxle->setText(QChar(IconIdentity::icon_location_arrow));

    initUIs();
}

RobotSport::~RobotSport()
{
    delete ui;
}



void RobotSport::on_pbAxle_clicked()
{
    try {
        RobotAxle axle = RobotAxle::instance();
        axle.a1 = ui->le_jt1->text().toDouble() ;
        axle.a2 = ui->le_jt2->text().toDouble() ;
        axle.a3 = ui->le_jt3->text().toDouble() ;
        axle.a4 = ui->le_jt4->text().toDouble() ;
        axle.a5 = ui->le_jt5->text().toDouble() ;
        axle.a6 = ui->le_jt6->text().toDouble() ;
        RobotHelper::GetInstance()->_robot->relativeMove_axle(axle) ;
        //RobotHelper::GetInstance()->_robo
    }
    catch( const boost::exception &e ) {
        DUMPERROR(e) ;
    }
}

void RobotSport::on_pbPos_clicked()
{
    try {
        RobotPos pos = RobotPos::instance();
        pos.x = ui->le_rtx->text().toDouble() ;
        pos.y = ui->le_rty->text().toDouble() ;
        pos.z = ui->le_rtz->text().toDouble() ;
        pos.a = ui->le_rta->text().toDouble() ;
        pos.b = ui->le_rtb->text().toDouble() ;
        pos.c = ui->le_rtc->text().toDouble() ;
        RobotHelper::GetInstance()->RelativeMove(pos,SysMembers::GetInstance()->debugRobotSpeed);
    }
    catch( const boost::exception &e ) {
        DUMPERROR(e) ;
    }
}

void RobotSport::on_RxyzMove_clicked()
{
    RobotPos b2b = RobotPos::instance();
    b2b.x = ui->dx->text().toDouble();
    b2b.y = ui->dy->text().toDouble();
    b2b.z = ui->dz->text().toDouble();
    b2b.c = ui->drx->text().toDouble();
    b2b.b = ui->dry->text().toDouble();
    b2b.a = ui->drz->text().toDouble();
    Matrix4d mb2b;
    pos2Matrix(b2b,mb2b,zyx);
    Matrix2pos(mb2b,b2b,zyz);
    auto pos = RobotHelper::GetInstance()->getCurrentPos()<<b2b;
    RobotHelper::GetInstance()->ABSMove(pos,SysMembers::GetInstance()->debugRobotSpeed);
}

void RobotSport::on_pb_run_clicked()
{
    std::thread([this](){
//        /* versionA: */
//        for(int i = 1; i < dataPack.sportCount + 1 ;i++){
//            std::string finm = "rundata/mult/" + dataPack.fileName + "_" + to_string(i);
//            RobotHelper::GetInstance()->MultMove(dataPack.pmap[i],finm);
//            sleep_s(2);
//            RobotHelper::GetInstance()->ABSMove(dataPack.pos,SysMembers::GetInstance()->debugRobotSpeed);
//        }

        std::ofstream log;
        /* versionB: */
        for(int i = 1; i < dataPack.sportCount + 1 ;i++){
            std::vector<double> ttms;
            std::string finm = "rundata/mult/" + dataPack.fileName + "_" + to_string(i);
            log.open(finm+".rep",ios_base::out);
            for(auto y:dataPack.pmap[i]){
                auto value = 400+random() % 800;
                ttms.push_back(value);
                log<< value <<endl;
            }

            // RobotHelper::GetInstance()->_robot->MultMove(dataPack.pmap[i],ttms,finm);
            sleep_s(2);
            RobotHelper::GetInstance()->ABSMove(dataPack.pos,SysMembers::GetInstance()->debugRobotSpeed);
        }
        log.close();
    }).detach();

}

void RobotSport::on_pb_Gen_clicked()
{
    initDataPack();
    dataPack.genVect(getRadioGroupIndex() == 1);
    dataPack.genName();
    dataPack.saveToLog();
    ui->pb_run->setEnabled(true);
    for(auto p:dataPack.pmap[1]){
        std::cout << p.toStr() << std::endl;
    }
}

void RobotSport::initDataPack()
{
    if(ui->le_pos->text().isEmpty()){
        QMessageBox::warning(this, tr("Warning"),"没有填充pos",QMessageBox::Cancel);
    }
    auto spos = ui->le_pos->text().toStdString();
    dataPack.pos = RobotPos::instance(spos);
    dataPack.isBase = ( 0 == ui->cb_cood->currentIndex() );
    dataPack.move = (MultPos::Direction) ui->cb_direct->currentIndex();
    dataPack.smove = ui->cb_direct->currentText().toStdString();
    dataPack.genCount = ui->sp_allCount->value();
    dataPack.spacing = ui->sp_spacing->value();
    dataPack.endSpeed = 100 ;
    dataPack.sportCount = ui->sp_runCOunt->value();
    dataPack.holdSpeed = ui->sp_hold->value();
}

// 获取checkbox索引
int RobotSport::getRadioGroupIndex()
{
    return abs(radios->checkedId() + 2 ) ;
}

void RobotSport::initUIs()
{
    radios = new QButtonGroup;
    radios->addButton(ui->rb_mut);
    radios->addButton(ui->rb_hold);
    radios->setExclusive(true);
    ui->rb_hold->setChecked(true);
}

void MultPos::DataPackage::genVect(bool isHold)
{
    for(int i = 1; i < this->sportCount + 1 ;i++){
        if(isHold){
            auto ff = MultPos::GenPos(this->isBase,this->move,this->spacing,this->pos,this->genCount);
            for(auto it = ff.begin();it!=ff.end();it++){
                (*it).v = holdSpeed;
                (*it).acc = 80;
            }
            pmap[i]=(ff);
        }else{
            for(int speed = this->startSpeed; speed < this->endSpeed + 1 ;speed+=10){
                auto ff = MultPos::GenPos(this->isBase,this->move,this->spacing,this->pos,this->genCount);
                for(auto it = ff.begin();it!=ff.end();it++){
                    (*it).v = speed;
                    (*it).acc = 80;
                }
                pmap[i]=(ff);
            }
        }

    }
}

void MultPos::DataPackage::saveToLog()
{
    std::ofstream log;

    log.open("rundata/tubeMove/" + this->fileName,ios_base::out);
    log <<  "ID" << "\t"  <<  "Direct" << "\t" << "Spacing" << "\t" << "Speed" << "\t" << "Pos" << endl;

    // 全部都遍历
//    for(auto it = pmap.begin();it!=pmap.end();it++){

//    }
    // 反正都是相同的，用第一个就行
    auto it = pmap.begin();
    auto _poss = it->second;
    int  index = it->first;
    for(auto pp:_poss){
        log <<  index << "\t";
        log <<  this->smove << "\t";
        log <<  to_string(this->spacing) << "\t";
        log <<  to_string(pp.v) << "\t";
        log <<  to_string(this->spacing) << "\t";
        log <<  pp.toStr() ;
        log <<  endl;
    }
    log.close();
}

void MultPos::DataPackage::genName()
{
    std::string base = this->isBase ? "_Base" : "_Tool";
    fileName =  this->smove + "_" + to_string(this->spacing) +  base;
}

std::vector<RobotPos> MultPos::DataPackage::getAllPos()
{
    std::vector<RobotPos> ret;

//    auto it = pmap.begin();
//    for(auto var:it->second){
//        for(auto pp:var){
//            ret.push_back(pp);
//        }
//    }

    return ret;
}

void RobotSport::on_pb_SP_clicked()
{
    double sp = ui->dsb_sp->value();
    RobotHelper::GetInstance()->_robot->setSpeed(sp,80);
}
